#ifndef __COMM_BASE_EXTEND_H__
#define __COMM_BASE_EXTEND_H__

#include <float.h>
#include <deque>
#include <mutex>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include "comm/comm_base.h"

/* Eigen 相关基本数据类型　*/
typedef float TMPLT;
typedef Eigen::Matrix<TMPLT, 2, 1> Vector2t;
typedef Eigen::Matrix<TMPLT, 3, 1> Vector3t;
typedef Eigen::Matrix<TMPLT, 4, 1> Vector4t;
typedef Eigen::Matrix<TMPLT, 6, 1> Vector6t;

typedef Eigen::Matrix<TMPLT, 3, 3> Matrix3t;
typedef Eigen::Matrix<TMPLT, 4, 4> Matrix4t;
typedef Eigen::AngleAxis<TMPLT>    AngleAxist;
typedef Eigen::Quaternion<TMPLT>   Quaterniont;
typedef Eigen::Matrix<TMPLT, Eigen::Dynamic, 1> VectorXt;
typedef Eigen::Matrix<TMPLT, Eigen::Dynamic, Eigen::Dynamic> MatrixXt;

/* float型 */
typedef Eigen::Matrix<float, 2, 1> Vector2f;
typedef Eigen::Matrix<float, 3, 1> Vector3f;
typedef Eigen::Matrix<float, 4, 1> Vector4f;
typedef Eigen::Matrix<float, 6, 1> Vector6f;
typedef Eigen::Matrix<float, Eigen::Dynamic, 1> VectorXf;

typedef Eigen::Matrix<float, 3, 3> Matrix3f;
typedef Eigen::Matrix<float, 4, 4> Matrix4f;
typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> MatrixXf;

typedef Eigen::AngleAxis<float>    AngleAxisf;
typedef Eigen::Quaternion<float>   Quaternionf;

/* double型 */
typedef Eigen::Matrix<double, 2, 1> Vector2d;
typedef Eigen::Matrix<double, 3, 1> Vector3d;
typedef Eigen::Matrix<double, 4, 1> Vector4d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorXd;

typedef Eigen::Matrix<double, 3, 3> Matrix3d;
typedef Eigen::Matrix<double, 4, 4> Matrix4d;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MatrixXd;

typedef Eigen::AngleAxis<double>   AngleAxisd;
typedef Eigen::Quaternion<double>  Quaterniond;
typedef Eigen::Transform<double,3, Eigen::Isometry> Isometry3d;


/* PCL 相关基本数据类型　*/
typedef pcl::PointXYZI MyPoint;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<MyPoint> MyCloud;
typedef pcl::PointCloud<PointT>  MyCloudT;

float AngleNormalize(const float &angle);
void  Rot2Euler(const Matrix3t &R, TMPLT &roll, TMPLT &pitch, TMPLT &yaw);
Matrix3d Hat(const Vector3d &v);
Vector3d Vee(const Matrix3d &m);

typedef struct ATTITUDE_STRU {
    float roll;
    float pitch;
    float yaw;

    ATTITUDE_STRU(void){
        SetZero();
    }

    ATTITUDE_STRU(const float &srcRoll, const float &srcPitch, const float &srcYaw){
        SetValue(srcRoll, srcPitch, srcYaw);
    }

    void Copy(const ATTITUDE_STRU &stSrc){
        roll  = stSrc.roll; pitch = stSrc.pitch; yaw   = stSrc.yaw;
    }

    void Copy(const Matrix3t &R){
        SetValue(R);
    }

    void SetZero(void){
        SetValue(0.0, 0.0, 0.0);
    }

    void SetValue(const Matrix3t &R){
        Rot2Euler(R, roll, pitch, yaw);
        SetValue(roll, pitch, yaw);
    }

    void SetValue(const float &srcRoll, const float &srcPitch, const float &srcYaw){
        roll = srcRoll; pitch = srcPitch; yaw = srcYaw;
    }
    
    Matrix3t Matrix(void) const{
        Eigen::AngleAxisf rotation_x( roll,  Vector3t::UnitX());
        Eigen::AngleAxisf rotation_y( pitch, Vector3t::UnitY());
        Eigen::AngleAxisf rotation_z( yaw,   Vector3t::UnitZ());
        Matrix3t R = (rotation_z * rotation_y * rotation_x) * Matrix3t::Identity();

        return R;
    }

    Matrix3t MatrixInv(void) const{
        return Matrix().transpose();
    }

    Quaterniont Quat(void) const{
        Quaterniont quat;
        quat = Matrix();
        return quat;
    }

    float CalcMaxAttDiff(const ATTITUDE_STRU &stAnotherAtt) const{
        float diffRoll  = CalcAngleDiffAbs(roll,  stAnotherAtt.roll);  
        float diffPitch = CalcAngleDiffAbs(pitch, stAnotherAtt.pitch);
        float diffYaw   = CalcAngleDiffAbs(yaw,   stAnotherAtt.yaw); 
        return MAX( MAX(diffRoll, diffPitch), diffYaw);
    }
    
} ATTITUDE_STRU;

typedef struct POSE_DIFF_STRU {
    float dist;
    float angle;

    POSE_DIFF_STRU(void){
        dist  = FLT_MAX;
        angle = PI;
    }
} POSE_DIFF_STRU;

/* 3D位姿数据结构 */
typedef struct COOR_3D_POSE_STRU {
    COORDINATE_3D_STRU stPos;
    ATTITUDE_STRU      stAtt;

    COOR_3D_POSE_STRU(void){
        SetZero();
    }

    COOR_3D_POSE_STRU(const float srcX,    const float srcY,     const float srcZ, 
                      const float srcRoll, const float srcPitch, const float srcYaw){
        SetValue(srcX, srcY, srcZ, srcRoll, srcPitch, srcYaw);
    }

    COOR_3D_POSE_STRU(const COORDINATE_3D_STRU &stSrcPos, 
                      const ATTITUDE_STRU      &stSrcAtt){
        SetValue(stSrcPos.x,    stSrcPos.y,     stSrcPos.z, 
                 stSrcAtt.roll, stSrcAtt.pitch, stSrcAtt.yaw);
    }

    COOR_3D_POSE_STRU(const Matrix4t &mPose){
        stAtt.SetValue(mPose.block(0,0,3,3));
        stPos.SetValue(mPose(0,3), mPose(1,3),  mPose(2,3));
    }

    void Copy(const COOR_3D_POSE_STRU &stSrc){
        SetValue(stSrc.stPos.x,    stSrc.stPos.y,     stSrc.stPos.z, 
                 stSrc.stAtt.roll, stSrc.stAtt.pitch, stSrc.stAtt.yaw);
    }

    void Copy(const Matrix4t &mPose){ 
        stAtt.SetValue(mPose.block(0,0,3,3));
        stPos.SetValue(mPose(0,3), mPose(1,3),  mPose(2,3));
    }

    void SetValue(const float &srcX,    const float &srcY,     const float &srcZ, 
                  const float &srcRoll, const float &srcPitch, const float &srcYaw){
        stPos.SetValue(srcX,    srcY,     srcZ);
        stAtt.SetValue(srcRoll, srcPitch, srcYaw);
    }

    void SetZero(void){
        SetValue(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
    }

    Matrix4t GetTransform(void) const{
        Matrix4t mPose = Matrix4t::Identity();
        mPose.block(0,3,3,1) = GetTranslation();
        mPose.block(0,0,3,3) = GetRotation();
        return mPose;
    }

    Matrix4t GetTransformInv(void) const{
        Matrix4t mPose = Matrix4t::Identity();
        mPose.block(0,3,3,1) = -GetRotation().transpose() * GetTranslation();
        mPose.block(0,0,3,3) =  GetRotation().transpose();
        return mPose;
    }

    Matrix3t GetRotation(void) const {
        return stAtt.Matrix();
    }

    Vector3t GetTranslation(void) const {
        return stPos.Vec();
    }

    float x(void) const {
        return stPos.x;
    }

    float y(void) const {
        return stPos.y;
    }

    float z(void) const {
        return stPos.z;
    }

    float roll(void) const {
        return stAtt.roll;
    }

    float pitch(void) const {
        return stAtt.pitch;
    }

    float yaw(void) const {
        return stAtt.yaw;
    }

    POSE_DIFF_STRU CalcPoseDiff(const Matrix4t &mAnotherPose) const {
        POSE_DIFF_STRU stPoseDiff;
        Matrix4t   mDiffPose = GetTransformInv() * mAnotherPose;
        AngleAxist diffAngleAxis(mDiffPose.block<3, 3>(0, 0));
        stPoseDiff.dist  = mDiffPose.block<3, 1>(0, 3).norm();
        stPoseDiff.angle = diffAngleAxis.angle();

        return stPoseDiff;
    }

    POSE_DIFF_STRU CalcPoseDiff(const COOR_3D_POSE_STRU &stAnotherPose) const {
        return CalcPoseDiff(stAnotherPose.GetTransform());
    }

    BOOL IsAboutEqual(const COOR_3D_POSE_STRU &stAnotherPose) const {
        POSE_DIFF_STRU stPoseDiff = CalcPoseDiff(stAnotherPose);
        if ( stPoseDiff.dist < 0.05 && stPoseDiff.angle < ANGLE_TO_RADIAN(0.5)){
            return true;
        }
        return false;
    }

} COOR_3D_POSE_STRU;


/* 传感器相关数据结构　*/
typedef struct CLOUD_DATA_STRU{
    double       time;
    MyCloud::Ptr pPoints;

    CLOUD_DATA_STRU(){
        time = 0.0;
        pPoints.reset(new MyCloud());
    };  
} CLOUD_DATA_STRU;

typedef struct VEL_DATA_STRU{
    double      time;
    Vector3t    linearVel;
    Vector3t    angleVel;

    VEL_DATA_STRU(void){
        SetZero();
    }

    VEL_DATA_STRU(const VEL_DATA_STRU &stSrcVelData){
        SetValue(stSrcVelData.time, stSrcVelData.linearVel, stSrcVelData.angleVel);
    }

    void Copy(const VEL_DATA_STRU &stSrcVelData){
        SetValue(stSrcVelData.time, stSrcVelData.linearVel, stSrcVelData.angleVel);
    }

    void SetZero(void){
        SetValue(0.0, Vector3t(0.0, 0.0, 0.0), Vector3t(0.0, 0.0, 0.0));
    }

    void SetValue(const double srcTime, const Vector3t &srcLinearVel, const Vector3t &srcAngleVel){
        time      = srcTime;
        linearVel = srcLinearVel;
        angleVel  = srcAngleVel;
    } 
} VEL_DATA_STRU;

typedef struct WHEEL_VEL_DATA_STRU{
    double time;
    float  vel;
} WHEEL_VEL_DATA_STRU;

typedef struct ODOM_DATA_STRU{
    double             time;
    COOR_3D_POSE_STRU  stPose;
    VEL_DATA_STRU      stVelData;

    ODOM_DATA_STRU(void){
        SetZero();
    }

    void Copy(const ODOM_DATA_STRU &stOdomData)
    {
        SetValue(stOdomData.time, stOdomData.stPose, stOdomData.stVelData);
    }
    
    void SetZero(void)
    {
        time = 0;;
        stPose.SetZero();
        stVelData.SetZero();
    }

    void SetValue(const double &srcTime, const COOR_3D_POSE_STRU &stSrcPose, const VEL_DATA_STRU &stSrcVelData)
    {
        time = srcTime;
        stPose.Copy(stSrcPose);
        stVelData.Copy(stSrcVelData);
    }
} ODOM_DATA_STRU;

typedef struct IMU_DATA_STRU{
    double      time;
    Vector3t    acc;
    Vector3t    gyro;
    Quaterniont orientation;

    IMU_DATA_STRU(void) {
        SetZero();
    }

    IMU_DATA_STRU(const IMU_DATA_STRU &stImuData) {
        SetValue(stImuData.time, stImuData.acc, stImuData.gyro, stImuData.orientation);
    }

    void Copy(const IMU_DATA_STRU &stImuData){
        SetValue(stImuData.time, stImuData.acc, stImuData.gyro, stImuData.orientation);
    }

    Matrix3t GetRotation(void) const {
        Matrix3t rotation = orientation.matrix();
        return rotation;
    }

    Vector3t GetEuler(void) const {
        Matrix3t rotation = GetRotation();

        Vector3t eulerAngle;
        Rot2Euler(rotation, eulerAngle(0), eulerAngle(1), eulerAngle(2));
        
        return eulerAngle;
    }

    Quaterniont GetQuat(void) const {
        return orientation;
    }

    void SetZero(void){
        SetValue(0.0, Vector3t(0.0, 0.0, 0.0), Vector3t(0.0, 0.0, 0.0), 
                 Quaterniont(1.0, 0.0, 0.0, 0.0));
    }
    
    void SetValue(const double   &srcTime, const Vector3t    &srcAcc, 
                  const Vector3t &srcGyro, const Quaterniont &srcQuat) {
        time = srcTime;
        acc  = srcAcc;
        gyro = srcGyro;
        orientation = srcQuat;
    }

} IMU_DATA_STRU;

typedef struct CTRL_INPUT_STRU
{
    double     time;
    Vector6f   ctrlInput;

    CTRL_INPUT_STRU(void)
    {
        time      = 0.0;
        ctrlInput = Vector6f::Zero();
    }

    CTRL_INPUT_STRU Interpolation(const double &xTime, const CTRL_INPUT_STRU &stCtrlInputE) const
    {
        double deltaT     = stCtrlInputE.time - time;
        float  ratio      = deltaT < 0.005 ? 0.5 : (xTime - time) / deltaT;

        CTRL_INPUT_STRU   stCtrlInputX;
        stCtrlInputX.time      = xTime;
        stCtrlInputX.ctrlInput = (1 - ratio) * ctrlInput + ratio * stCtrlInputE.ctrlInput;
        
        return stCtrlInputX;     
    }

} CTRL_INPUT_STRU;

typedef struct TIMESTAMP_POSE_STRU
{ 
    double            time;
    COOR_3D_POSE_STRU stPose;

    TIMESTAMP_POSE_STRU Interpolation(const double &xTime, const TIMESTAMP_POSE_STRU &stTimePoseE) const
    {
        Matrix4t mPoseX   = Matrix4t::Identity();
        double deltaT     = stTimePoseE.time - time;
        float  ratio      = deltaT < 0.005 ? 0.5 : (xTime - time) / deltaT;
        Vector3t vPosX    = (1 - ratio)*stPose.GetTranslation()  + ratio*stTimePoseE.stPose.GetTranslation();
        Quaterniont quatX = stPose.stAtt.Quat().slerp(ratio, stTimePoseE.stPose.stAtt.Quat());
        mPoseX.block<3,1>(0,3) = vPosX;
        mPoseX.block<3,3>(0,0) = quatX.normalized().matrix();

        TIMESTAMP_POSE_STRU  stTimePoseX;
        stTimePoseX.time = xTime;
        stTimePoseX.stPose.Copy(mPoseX);
        
        return stTimePoseX;     
    }
} TIMESTAMP_POSE_STRU;

typedef struct MOVE_INFO_DATA_STRU{
    double   time;
    float    steeringAngle; 
    float    moveLinearVel;
    float    yawAngle;
    float    yawAngleVel;
    Vector3t acc;

    void Copy(const MOVE_INFO_DATA_STRU &stMoveInfo){
        time          = stMoveInfo.time;
        steeringAngle = stMoveInfo.steeringAngle; 
        moveLinearVel = stMoveInfo.moveLinearVel;
        yawAngle      = stMoveInfo.yawAngle;
        yawAngleVel   = stMoveInfo.yawAngleVel;
        acc           = stMoveInfo.acc;
    }

} MOVE_INFO_DATA_STRU;

typedef struct POSE_SCORE_STRU{
    float     score;
    Matrix4t  mPose;

    POSE_SCORE_STRU(void) {
        score = DBL_MAX;
        mPose = Matrix4t::Identity();
    } 

    void Copy(POSE_SCORE_STRU &stPoseScore)
    {
       score = stPoseScore.score;
       mPose = stPoseScore.mPose;
    }
    
} POSE_SCORE_STRU;

template<class T> class DATA_CHANNEL_C 
{
public:
    DATA_CHANNEL_C (INT32 maxSize = 10000) : maxSize_(maxSize)
    {
       vItems_.clear();
    };

    void Send(const T &item)
    {
        std::lock_guard<std::mutex> lock(mutex_);
        vItems_.push_back(item);
        while (vItems_.size() > maxSize_){
            vItems_.pop_front();
        }
    }

    INT32 Get(std::deque<T> &vItems)
    {
        std::lock_guard<std::mutex> lock(mutex_);
        if (vItems_.size() > 0) {
            vItems.insert(vItems.end(), vItems_.begin(), vItems_.end());
            vItems_.clear();
            return 0;
        }
        return -1;
    }

private:
    size_t         maxSize_;
    std::deque<T>  vItems_;
    std::mutex     mutex_;
};


#endif
